
# 20211005 GiBa: added more functions to buttons

# HUB import
from pybricks.hubs import TechnicHub

# Remote import
from pybricks.pupdevices import Remote
from pybricks.parameters import Button
from pybricks.parameters import Color

# Motor import 
from pybricks.pupdevices import Motor
from pybricks.parameters import Port
from pybricks.parameters import Stop

# Led light
from pybricks.pupdevices import Light

# Tool import
from pybricks.tools import wait

# Initialize HUB
#print(Color.GREEN.h, Color.GREEN.s, Color.GREEN.v) # 120 100 100
#print(Color.YELLOW.h, Color.YELLOW.s, Color.YELLOW.v) #60 100 100
#print(Color.ORANGE.h, Color.ORANGE.s, Color.ORANGE.v) #30 100 100
#print(Color.RED.h, Color.RED.s, Color.RED.v) #0 100 100 
durationList = [75, 75, 75, 75, 75, 1000]

hub = TechnicHub()
hub.light.blink(Color.WHITE, durationList)

# Initialize Led light
led = Light(Port.C)
led.off()
ledStatus = 0 # 0: off, 1: on
oldLedStatus = 0

# Connect to the remote
remote = Remote()
remote.light.on(Color.YELLOW)
hub.light.on(Color.YELLOW)

# Initialize a motor on port A and B
steeringMotor = Motor(Port.B)
driveMotor = Motor(Port.A)
maxDriveSpeed = 1250 # experimental, TODO: add autotune

# Set parameters to steering motor to be more responsible
#print(steeringMotor.control.target_tolerances()) # Read default value
steeringMotor.control.target_tolerances(5, 1) # Default 50, 10
#print(steeringMotor.control.limits()) # Read default value
steeringMotor.control.limits(1000, 10000, 100, 520) # Default: 1000, 1500, 100, 260
#print(steeringMotor.control.pid()) # Read default value
steeringMotor.control.pid(40000, 600, 2000, 450, 50) # Default: 4000, 600, 1000, 45, 5

# Center steering motor
steringSpeed = 200 # Centering speed
angleRight = steeringMotor.run_until_stalled(-steringSpeed, duty_limit=60, then=Stop.HOLD)
angleLeft = steeringMotor.run_until_stalled(steringSpeed, duty_limit=60, then=Stop.HOLD)
angleZero = (angleLeft + angleRight) / 2
steeringMotor.run_target(steringSpeed, angleZero, then=Stop.HOLD , wait=False)

steringSpeed = 1000 # Default steering speed
wait(1000)

# Define power for drive motor
actualSpeed = 0
colorHueValue = 120
colorSpeed = Color(h=colorHueValue, s=100, v=100) # GREEN
maxPower = 100
idlePower = 80

powerState = 0 #0: null, 1: break, 2: maxPower, 3: coast
powerDirection = 0
oldPowerState = 0
oldPowerDirection = 0

# Ready!
remote.light.on(Color.GREEN)
hub.light.on(Color.GREEN)

# Buttons status
statusLeftPlus = False;
statusLeftMinus = False;
statusRightPlus = False;
statusRightMinus = False;

statusLeft = False;
statusRight = False;

oldStatusLeftPlus = False;
oldStatusLeftMinus = False;
oldStatusRightPlus = False;
oldStatusRightMinus = False;

oldStatusLeft = False;
oldStatusRight = False;

while True:
    # Get actual speed and calculate led color to a smooth change between them
    actualSpeed = abs(driveMotor.speed())

    if actualSpeed > maxDriveSpeed:
        maxDriveSpeed = actualSpeed

    colorHueValue = 120 - 120 * actualSpeed / (maxDriveSpeed - 10)
    
    if colorHueValue < 0:
        colorHueValue = 0

    if colorHueValue > 120:
        colorHueValue = 120

    # Check which buttons are pressed
    pressed = remote.buttons.pressed()

    if Button.LEFT_PLUS in pressed:
        statusLeftPlus = True
    else:
        statusLeftPlus = False

    if Button.LEFT_MINUS in pressed:
        statusLeftMinus = True
    else:
        statusLeftMinus = False

    if Button.RIGHT_PLUS in pressed:
        statusRightPlus = True
    else:
        statusRightPlus = False

    if Button.RIGHT_MINUS in pressed:
        statusRightMinus = True
    else:
        statusRightMinus = False

    if Button.RIGHT in pressed:
        statusRight = True
    else:
        statusRight = False

    if Button.LEFT in pressed:
        statusLeft = True
    else:
        statusLeft = False

    # Steering commands only if there is a button change
    if statusLeftPlus != oldStatusLeftPlus:
        if statusLeftPlus == True:
            #print("You pressed the left plus button!")
            steeringMotor.run_target(steringSpeed, angleRight + 1, then=Stop.HOLD, wait=False)
        else:
            #print("You released the left plus button!")
            steeringMotor.run_target(steringSpeed, angleZero, then=Stop.HOLD, wait=False)

    if statusLeftMinus != oldStatusLeftMinus:
        if statusLeftMinus == True:
            steeringMotor.run_target(steringSpeed, angleLeft - 1, then=Stop.HOLD, wait=False)
            #print("You pressed the left minus button!")
        else:
            #print("You released the left minus button!")
            steeringMotor.run_target(steringSpeed, angleZero, then=Stop.HOLD, wait=False)

    # Power commands
    if statusLeft != oldStatusLeft:
        if statusRight == False:
            # Update led light on button released
            if ledStatus == 0:
                ledStatus = 1
            else:
                ledStatus = 0

    # Power commands
    if statusRight != oldStatusRight:
        if statusRight == True:
            # print("You pressed the right button!")
            powerState += 1
        else:
            #print("You released the right button!")
            powerState -= 1

    if statusRightPlus != oldStatusRightPlus:
        if statusRightPlus == True:
            #print("You pressed the right plus button!")
            powerState += 1
            powerDirection = -1
        else:
            #print("You released the right plus button!")
            powerState -= 1
            powerDirection = 0

    if statusRightMinus != oldStatusRightMinus:
        if statusRightMinus == True:
            #print("You pressed the right minus button!")
            powerState += 1
            powerDirection = 1
        else:
            #print("You released the right minus button!")
            powerState -= 1
            powerDirection = 0

    if powerState != oldPowerState or powerDirection != oldPowerDirection:
        if powerDirection == 0:
            if powerState == 0:
                driveMotor.stop()
                #print("*** MOTOR COAST ***")
            else:
                driveMotor.brake()
                #print("*** MOTOR BRAKE ***")
        else: # power direction is not zero
            if powerState == 1:
                driveMotor.dc(powerDirection * idlePower)
                #print("*** MOTOR IDLE ***")
            elif powerState == 2:
                driveMotor.dc(powerDirection * maxPower)
                #print("*** MOTOR MAX ***")

    if powerState == 1 and powerDirection == 0: # BRAKE
        colorSpeed = Color(359, s=100, v=25)
        hub.light.on(colorSpeed)
        remote.light.on(colorSpeed)
    else:
        colorSpeed = Color(h=colorHueValue, s=100, v=100)
        hub.light.on(colorSpeed)
        remote.light.on(colorSpeed)

    # Update led on/off
    if ledStatus != oldLedStatus:
        if ledStatus == 0:
            led.off()
        else:
            led.on(100)

    # Update status
    oldStatusLeftPlus = statusLeftPlus
    oldStatusLeftMinus = statusLeftMinus
    oldStatusRightPlus = statusRightPlus
    oldStatusRightMinus = statusRightMinus
    oldStatusRight = statusRight
    oldStatusLeft = statusLeft
    oldPowerState = powerState
    oldPowerDirection = powerDirection
    oldLedStatus = ledStatus

